/**
 * @file Strategy.cpp
 * @brief 决策器的实现，接收识别器推理得到的检测物体集合，对检测到
 *        的物体数据进行解析，处理数据并做出最终决策
 * @author Rosen (1018477962@qq.com)
 * @version 1.0
 * @date 2021-09-26
 * 
 * @copyright Copyright (c) 2021 by Rosen.
 */

#include "Strategy.h"

/**
 * @brief  决策器执行分析
 * @param  objects  检测出的对象
 * @param  frame    rgb源图像
 * @param  depth    深度原图像
 */
void Strategy::analysis(std::vector<Object> objects)
{
    this->_objects = objects;

    // 深度图为空则未连接相机，决策器停止工作
    if (objects.empty())
        return;

    // 分类
    this->_classific();
    // 获取目标
    this->_goal = this->_obtainGoal();

    // 发现目标则计算欧拉角和世界坐标，否则清空
    if (this->isDiscover)
    {
        this->_eulerAngle = this->_calcRelativeAngle(cameraParam.cameraMatrix, this->_goal.center);
        this->_calcObjectCoor(this->_eulerAngle, this->_goal.depth);
    }
    else
    {
        this->_eulerAngle.clear();
        this->_goalObjectCoor = cv::Point3f(0.f, 0.f, 0.f);
    }
}

/**
 * @brief 对检测到的物体进行分类
 */
void Strategy::_classific()
{
    this->_Mature.clear();
    this->_Midmature.clear();
    this->_Immature.clear();

    // 根据target判断分类
    for (auto object : this->_objects)
    {
        if (object.target == datasetParam.className[0])
            this->_Mature.push_back(object);
        else if (object.target == datasetParam.className[1])
            this->_Midmature.push_back(object);
        else if (object.target == datasetParam.className[2])
            this->_Immature.push_back(object);
    }
    // 检验
    size_t size = this->_Mature.size() + this->_Midmature.size() + this->_Immature.size();
    assert(size == this->_objects.size());
}

/**
 * @brief 在检测到的成熟番茄中检测目标
 * @return Object 范围内的抓取目标
 */
Object Strategy::_obtainGoal()
{
    Object goal;
    this->isDiscover = false;
    // 根据目标深度进行排序
    sort(this->_Mature.begin(), this->_Mature.end(), [&](Object t1, Object t2) -> bool
         { return t1.depth < t2.depth; });
    // 选取小于最远距离的最近成熟番茄为目标
    if (!this->_Mature.empty() && this->_Mature.front().depth < strategyParam.maxDis)
    {
        goal = this->_Mature.front();
        this->isDiscover = true;
    }
    return goal;
}

/**
 * @brief 计算世界坐标系坐标
 * @param  relativeAngle 相对角度
 * @param  depth    深度 --单位为mm
 */
void Strategy::_calcObjectCoor(const eulerAngle &angle, const uint16_t &depth)
{
    float x, y, z;
    x = depth * sinf(angle.yaw);
    y = depth * sinf(angle.pitch);
    z = depth;

    this->_goalObjectCoor = cv::Point3f(x, y, z);
}

/**
 * @brief 用来获得相机中心相对角度
 * 
 * @param cameraMatrix 相机内参
 * @param center 图像中装甲板中心
 * @return x,y方向夹角 -- Point2f仅做储存，没有“点”的意义
 */
eulerAngle Strategy::_calcRelativeAngle(const cv::Mat &cameraMatrix, const cv::Point2f &center)
{
    cv::Mat tf_point(3, 1, CV_32F);
    cv::Mat cameraMatrix_inverse;
    // 矩阵转置
    cameraMatrix.convertTo(cameraMatrix_inverse, CV_32F);
    cameraMatrix_inverse = cameraMatrix_inverse.inv();

    tf_point.at<float>(0) = center.x;
    tf_point.at<float>(1) = center.y;
    tf_point.at<float>(2) = 1;
    // 得到tan角矩阵
    cv::Mat tf_result = cameraMatrix_inverse * tf_point;

    float yaw = atanf(tf_result.at<float>(0)) / PI * 180.f;
    float pitch = atanf(tf_result.at<float>(1)) / PI * 180.f;
    return eulerAngle{yaw, pitch};
}

void Strategy::info()
{
    std::cout << "***********************Begin*********************" << std::endl;
    std::cout << "Object Quantity:\t" << this->_objects.size() << std::endl;
    std::cout << "Mature Object Quantity:\t" << this->_Mature.size() << std::endl;
    std::cout << "Goal RelativeAngle:\t" << cv::Point2f(this->_eulerAngle.yaw, this->_eulerAngle.pitch) << std::endl;
    std::cout << "Goal ObjectCoor:\t" << this->_goalObjectCoor << std::endl;
    std::cout << "************************end**********************" << std::endl;
}
